/*=+--+=#=+--     Unmanned Aerial System Management Software      --+=#=+--+=#*\
|          Copyright (C) 2011 Regents of the University of Colorado.           |
|                             All Rights Reserved.                             |

     This program is free software: you can redistribute it and/or modify
     it under the terms of the GNU General Public License version 2 as
     published by the Free Software Foundation.

     This program is distributed in the hope that it will be useful,
     but WITHOUT ANY WARRANTY; without even the implied warranty of
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     GNU General Public License for more details.

     You should have received a copy of the GNU General Public License
     along with this program.  If not, see <http://www.gnu.org/licenses/>.

            Jack Elston                       Cory Dixon                        
|           elstonj@colorado.edu              dixonc@colorado.edu              |
|                                                                              |
\*=+--+=#=+--                 --+=#=+--+=#=+--                    --+=#=+--+=#*/

/***********************************************************************
 *
 * FILENAME:
 * matrix.cxx
 *
 * PURPOSE:
 *   To implement any needed Matrix functions, such as transormations.
 * Matrices can be read form and saved to a file.  This class contains
 * code for most of the known Matrix functions.
 *
 * CREATED:
 * 02/2005 by Cory Dixon
 *
 * LAST MODIFIED:
 * $Author: dixonc $
 * $Date: 2005/07/13 21:00:06 $
 * $Revision: 1.3 $
 *
 ***********************************************************************/

#include <iostream>
#include <iomanip>
#include <assert.h>
#include <fstream>
#include <stdio.h>
#include <math.h>
#include <string.h>
#include <limits>

#include "matrix.h"

#ifndef INFINITY
#	define INFINITY 0x7fffffff
#endif

// constructor
Matrix::Matrix (size_t row, size_t col, Matrix_t v)
{
	Row = row;
	Col = col;

	Val = new Matrix_t* [row];

	for (size_t i=0; i < row; i++)
	{
		Val[i] = new Matrix_t [col];
		for(size_t j=0; j < col; j++)
			Val[i][j] = v;
	}
}

Matrix::Matrix (size_t row, size_t col, Matrix_t v[])
{
	Row = row;
	Col = col;

	Val = new Matrix_t* [row];

	for (size_t i=0; i < row; i++)
	{
		Val[i] = new Matrix_t [col];
		for(size_t j=0; j < col; j++)
			Val[i][j] = v[i * col + j];
	}
}

Matrix::Matrix (size_t row, size_t col, float v[])
{
	Row = row;
	Col = col;

	Val = new Matrix_t* [row];

	for (size_t i=0; i < row; i++)
	{
		Val[i] = new Matrix_t [col];
		for(size_t j=0; j < col; j++)
			Val[i][j] = v[i * col + j];
	}
}
// copy constructor
Matrix::Matrix (const Matrix& m)
{
	Row = m.Row;
	Col = m.Col;

	Val = new Matrix_t* [Row];

	for (size_t i=0; i < Row; i++)
	{
		Val[i] = new Matrix_t [Col];
		memcpy( Val[i], m.Val[i], Col * sizeof(Matrix_t) );
	}

}

// file constructor
Matrix::Matrix (char *fileName) 
{
	Row = Col = 0;
	Val = NULL;

	if( fileName != NULL )
		(void)fillFromFile( fileName );
}


// destructor
Matrix::~Matrix ()
{
	for (size_t i=0; i < Row; i++)
		delete [] Val[i];
	delete [] Val;

}

// assignment operator
Matrix& Matrix::operator = (const Matrix_t inputArray[])
{
	for (size_t i=0; i < Row; i++)
		for (size_t j=0; j < Col; j++)
			Val[i][j] = inputArray[i*Col + j];

	return *this;
}

// assignment operator
Matrix& Matrix::operator = (const Matrix& m) 
{
	realloc(m.Row, m.Col);
	for (size_t i=0; i < Row; i++)
		memcpy( Val[i], m.Val[i], Col * sizeof(Matrix_t) );

	return *this;
}

//  reallocation method
void Matrix::realloc (size_t row, size_t col)
{
	if (row == Row && col == Col)
		return;

	// get new memory block
	Matrix_t** tempVal;
	tempVal = new Matrix_t* [row];

	for (size_t i=0; i < row; i++)
		tempVal[i] = new Matrix_t [col];

	size_t colSize = RET_MIN(Col,col) * sizeof(Matrix_t);
	size_t minRow = RET_MIN(Row,row);

	// copy values from old memory block
	for (size_t i=0; i < minRow; i++)
		memcpy( tempVal[i], Val[i], colSize);

	// delete old memory block
	for (size_t i=0; i < Row; i++)
		delete [] Val[i];
	delete [] Val;

	// set memory block to new block
	Val = tempVal;
	Row = row;
	Col = col;

	return;
}

// public method for resizing Matrix
void Matrix::SetSize (size_t row, size_t col) 
{
	size_t i,j;
	size_t oldRow = Row;
	size_t oldCol = Col;

	if (row != Row || col != Col)
		realloc( row, col);

	for (i=oldRow; i < row; i++)
		for (j=0; j < col; j++)
			Val[i][j] = Matrix_t(0);

	for (i=0; i < row; i++)                      
		for (j=oldCol; j < col; j++)
			Val[i][j] = Matrix_t(0);

	return;
}


// -------- Matrix_t (row,col) --------
// subscript operator to set individual elements
Matrix_t& Matrix::operator () (size_t row, size_t col) 
{
	//cout << "Row=" << Row << " row=" << row << "  "
		//<< "Col=" << Col << " col=" << col << endl;
	assert(row < Row && col < Col);
	return Val[row][col];
}

// subscript operator to get individual elements
Matrix_t Matrix::operator () (size_t row, size_t col) const 
{
	// check index
	assert(row < Row && col < Col);
	return Val[row][col];
}

// -------- Get vector from data ---------
void Matrix::getVec(Matrix_t vec[])
{
	assert(vec != NULL);	

	for( size_t ind = 0; ind < Col*Row; ind++) {
		vec[ind] = (*this)(ind);
	}

}

// -------- Row(ind) --------
void Matrix::getRow(size_t ind, Matrix_t vec[])
{
	assert(ind < Row && vec != NULL);	

	for( size_t col = 0; col < Col; col++) {
		vec[col]=Val[ind][col];
	}

}
Matrix Matrix::getRow(size_t ind)
{
	assert(ind < Row);

	Matrix m(1,Col);
	for( size_t col = 0; col < Col; col++) {
		m(0,col)=Val[ind][col];
	}
	return m;
}

void Matrix::setRow(size_t ind, Matrix vec)
{
	assert(ind < Row && vec.Col >= Col);	
	for( size_t col = 0; col < Col; col++)
		Val[ind][col] = vec.Val[0][col];
}

void Matrix::setRow(size_t ind, Matrix_t vec[])
{
	assert(ind < Row && vec != NULL);	

	for( size_t col = 0; col < Col; col++)
		Val[ind][col] = vec[col];
}

void Matrix::setRow(size_t ind, Matrix_t val)
{
	assert(ind < Row);

	for( size_t col = 0; col < Col; col++)
		Val[ind][col] = val;
}


// -------- Col(ind) --------
void Matrix::getCol(size_t ind, Matrix_t *vec)
{
	assert(ind < Col && vec != NULL);	

	for( size_t row = 0; row < Row; row++)
		 vec[row] = Val[row][ind];
}

Matrix Matrix::getCol(size_t ind)
{
	assert(ind < Col);

	Matrix m(Row,1);
	for( size_t row = 0; row < Row; row++)
		 m(row,0) = Val[row][ind];

	return m;
}

void Matrix::setCol(size_t ind, Matrix_t *vec)
{
	assert(ind < Col && vec != NULL);	

	for( size_t row = 0; row < Row; row++)
		Val[row][ind] = vec[row];
}

void Matrix::setCol(size_t ind, Matrix vec)
{
	assert(ind < Col && vec.Row >= Row);
	for( size_t row = 0; row < Row; row++)
		Val[row][ind] = vec.Val[row][0];
}

void Matrix::setCol(size_t ind, Matrix_t val)
{
	assert(ind < Col);
	for( size_t row = 0; row < Row; row++)
		Val[row][ind] = val;
}

// -------- (index) --------
// subscript operator to set individual elements
Matrix_t& Matrix::operator () (size_t ind) 
{
	size_t row, col;
	// check index
	assert(ind < Col*Row);

	if( Row > Col ) { 
		col = ind%Col;
		row = ind - col;
	} else {
		row = ind%Col;
		col = ind - row;
	}

	return Val[row][col];
}

// subscript operator to get individual elements
Matrix_t Matrix::operator () (size_t ind) const 
{
	size_t row, col;
	// check index
	assert(ind < Col*Row);

	if( Row > Col ) { 
		col = ind%Col;
		row = ind - col;
	} else {
		row = ind%Col;
		col = ind - row;
	}

	return Val[row][col];
}

// input stream function
istream& operator >> (istream& istrm, Matrix& m)
{
	for (size_t i=0; i < m.NumRow(); i++)
		for (size_t j=0; j < m.NumCol(); j++)
			istrm >> m(i,j);
	return istrm;
}

// output stream function
ostream& operator << (ostream& ostrm, const Matrix& m)
{
	ostrm.setf(ios::scientific | ios::left | ios::internal);

	for (size_t i=0; i < m.NumRow(); i++)
	{
		for (size_t j=0; j < m.NumCol(); j++)
		{
			ostrm << setw(15) 
				 << setprecision(7)
				 << m(i,j);
		}
		if( m.NumRow() > 1 )
			ostrm << endl;
	}
	// reset output format
	#if (defined( __GNUC__) && !defined(VXWORKS) && __GNUC__ >= 3)
		ostrm.setf( (std::_Ios_Fmtflags) 0, ios::floatfield);
	#else
		ostrm.setf( 0, ios::floatfield);
	#endif

	return ostrm;
}

// logical equal-to operator
bool Matrix::operator == (const Matrix& m1) const
{
	if (m1.NumRow() != Row || m1.NumCol() != Col)
		return false;

	for (size_t i=0; i < m1.NumRow(); i++)
		for (size_t j=0; j < m1.NumCol(); j++)
			if (m1(i,j) != Val[i][j])
				return false;

	return true;
}

// logical no-equal-to operator
bool Matrix::operator != (const Matrix& m1) const
{
	return !(m1 == *this);
}

// combined addition and assignment operator
Matrix& Matrix::operator += (const Matrix& m) 
{
	// check matrix sizes
	assert(Row == m.Row && Col == m.Col);

	for (size_t i=0; i < m.Row; i++)
		for (size_t j=0; j < m.Col; j++)
			Val[i][j] += m.Val[i][j];

	return *this;
}

// combined subtraction and assignment operator
Matrix& Matrix::operator -= (const Matrix& m) 
{
	assert(Row == m.Row && Col == m.Col);

	for (size_t i=0; i < m.Row; i++)
		for (size_t j=0; j < m.Col; j++)
			Val[i][j] -= m.Val[i][j];

	return *this;
}

// combined scalar multiplication and assignment operator
Matrix& Matrix::operator *= (const Matrix_t& c) 
{
	for (size_t i=0; i < Row; i++)
		for (size_t j=0; j < Col; j++)
			Val[i][j] *= c;

	return *this;
}

// combined Matrix multiplication and assignment operator
Matrix& Matrix::operator *= (const Matrix& m) 
{
	assert(Col == m.Row);

	Matrix temp(Row,m.Col);

	for (size_t i=0; i < Row; i++)
		for (size_t j=0; j < m.Col; j++)
		{
			temp.Val[i][j] = Matrix_t(0);
			for (size_t k=0; k < Col; k++)
				temp.Val[i][j] += Val[i][k] * m.Val[k][j];
		}

	*this = temp;

	return *this;
}

// combined scalar division and assignment operator

Matrix& Matrix::operator /= (const Matrix_t& c) 
{
	for (size_t i=0; i < Row; i++)
		for (size_t j=0; j < Col; j++)
			Val[i][j] /= c;

	return *this;
}

// combined power and assignment operator
Matrix& Matrix::operator ^= (const size_t& pow) 
{
	Matrix temp(*this);

	for (size_t i=2; i <= pow; i++)
		*this *= temp;

	return *this;
}

// unary negation operator
Matrix Matrix::operator - () const
{
	Matrix temp(Row,Col);

	for (size_t i=0; i < Row; i++)
		for (size_t j=0; j < Col; j++)
			temp.Val[i][j] = -(Val[i][j]);

	return temp;
}

// binary addition operator
Matrix Matrix::operator + (const Matrix& m2) const
{
	Matrix temp(*this);
	temp += m2;
	return temp;
}

// binary subtraction operator
Matrix Matrix::operator - (const Matrix& m2) const
{
	Matrix temp(*this);
	temp -= m2;
	return temp;
}

// binary scalar multiplication operator
Matrix Matrix::operator * ( const Matrix_t& no) const
{
	Matrix temp(*this);
	temp *= no;
	return temp;
}

// binary scalar multiplication operator
Matrix operator * (const Matrix_t& no, const Matrix& m) 
{
	return (m * no);
}

// binary Matrix multiplication operator
Matrix Matrix::operator * (const Matrix& m2) const
{
	Matrix temp(*this);
	temp *= m2;
	/*
	if( temp.NumRow() * temp.NumCol() == 1)
		return Matrix_t(temp(0,0));
	else
	*/
		return temp;
}

// binary scalar division operator
Matrix Matrix::operator / (const Matrix_t& no) const
{
	return ( (*this) * (Matrix_t(1) / no));
}


// binary scalar division operator
Matrix operator / (const Matrix_t& no, const Matrix& m) 
{
	return (!m * no);
}

// binary Matrix division operator
Matrix Matrix::operator / ( const Matrix& m2) const
{
	return ( (*this) * !m2);
}

// binary power operator
Matrix Matrix::operator ^ ( const size_t& pow) const
{
	Matrix temp(*this);
	temp ^= pow;
	return temp;
}

// unary transpose operator
Matrix Matrix::operator ~ () const
{
	Matrix temp(Col,Row);

	for (size_t i=0; i < Row; i++)
		for (size_t j=0; j < Col; j++)
			temp(j,i) = (*this)(i,j);

	return temp;
}

// unary inversion operator
Matrix Matrix::operator ! () const
{
	return this->Inv();
}

// inversion function
Matrix Matrix::Inv () const
{
	size_t i,j,k;
	Matrix_t a1,a2,*rowptr;

	// make sure its a square matrix
	assert(Row == Col);

	Matrix temp(Row,Col);
	Matrix thisCopy(*this);

	temp.Identity();
	for (k=0; k < Row; k++)
	{
		int indx = thisCopy.pivot(k);

		// make sure its not a singular Matrix
		assert(indx != -1);

		if (indx != 0)
		{
			rowptr = temp.Val[k];
			temp.Val[k] = temp.Val[indx];
			temp.Val[indx] = rowptr;
		}
		a1 = thisCopy.Val[k][k];
		for (j=0; j < Row; j++)
		{
			thisCopy.Val[k][j] /= a1;
			temp.Val[k][j] /= a1;
		}

		for (i=0; i < Row; i++)
			if (i != k)
			{
				a2 = thisCopy.Val[i][k];
				for (j=0; j < Row; j++)
				{
					thisCopy.Val[i][j] -= a2 * thisCopy.Val[k][j];
					temp.Val[i][j] -= a2 * temp.Val[k][j];
				}
			}
	}

	return temp;
}

// solve simultaneous equation
Matrix Matrix::Solve (const Matrix& v) const 
{
	size_t i,j,k;
	Matrix_t a1;

	// make sure dimensions match (this matrix must be square)
	assert(Row == Col && Col == v.Row);

	Matrix temp(Row,Col+v.Col);
	for (i=0; i < Row; i++)
	{
		for (j=0; j < Col; j++)
			temp.Val[i][j] = Val[i][j];
		for (k=0; k < v.Col; k++)
			temp.Val[i][Col+k] = v.Val[i][k];
	}
	for (k=0; k < Row; k++)
	{
		int indx = temp.pivot(k);
		// make sure its not singular
		assert(indx != -1);

		a1 = temp.Val[k][k];
		for (j=k; j < temp.Col; j++)
			temp.Val[k][j] /= a1;

		for (i=k+1; i < Row; i++)
		{
			a1 = temp.Val[i][k];
			for (j=k; j < temp.Col; j++)
				temp.Val[i][j] -= a1 * temp.Val[k][j];
		}
	}

	Matrix s(v.Row,v.Col);
	for (k=0; k < v.Col; k++)
		for (size_t m=int(Row)-1; m >= 0; m--)
		{
			s.Val[m][k] = temp.Val[m][Col+k];
			for (j=m+1; j < Col; j++)
				s.Val[m][k] -= temp.Val[m][j] * s.Val[j][k];
		}

	return s;
}

// set zero to all elements of this Matrix
void Matrix::Null (const size_t& row, const size_t& col) 
{
	if (row != Row || col != Col)
		realloc( row,col);

	for (size_t i=0; i < Row; i++)
		for (size_t j=0; j < Col; j++)
			Val[i][j] = Matrix_t(0);
	return;
}

// set zero to all elements of this Matrix
void Matrix::Null() 
{
	for (size_t i=0; i < Row; i++)
		for (size_t j=0; j < Col; j++)
			Val[i][j] = Matrix_t(0);

	return;
}
// set zero to all elements of this Matrix
void Matrix::Ones() 
{
	for (size_t i=0; i < Row; i++)
		for (size_t j=0; j < Col; j++)
			Val[i][j] = Matrix_t(1);

	return;
}
// set this Matrix to unity
Matrix& Matrix::Unit ()
{
	*this /= this->Norm();
	return *this;
}
bool Matrix::IsUnit () const
{
	return ( this->Norm() == 1 );
}

void Matrix::Identity (const size_t& row) 
{
	if (row != Row || row != Col)
		realloc( row, row);

	for (size_t i=0; i < Row; i++)
		for (size_t j=0; j < Col; j++)
			Val[i][j] = (i == j ? Matrix_t(1) : Matrix_t(0));
	return;
}

// set this Matrix to unity
void Matrix::Identity () 
{
	size_t row = RET_MIN(Row,Col);
	Row = Col = row;

	for (size_t i=0; i < Row; i++)
		for (size_t j=0; j < Col; j++)
			Val[i][j] = i == j ? Matrix_t(1) : Matrix_t(0);
	return;
}

// private partial pivoting method
int Matrix::pivot (size_t row)
{
	int k = int(row);
	double amax,temp;

	amax = -1;
	for (size_t i=row; i < Row; i++)
		if ( (temp = fabs( Val[i][row])) > amax && temp != 0.0)
		{
			amax = temp;
			k = i;
		}
	if (Val[k][row] == Matrix_t(0))
		return -1;
	if (k != int(row))
	{
		Matrix_t* rowptr = Val[k];
		Val[k] = Val[row];
		Val[row] = rowptr;
		return k;
	}
	return 0;
}

// calculate the determinant of a Matrix
Matrix_t Matrix::Det () const 
{
	size_t i,j,k;
	Matrix_t piv,detVal = Matrix_t(1);

	// make sure matrix is square
	assert(Row == Col);

	Matrix temp(*this);

	for (k=0; k < Row; k++)
	{
		int indx = temp.pivot(k);
		// singular matrix
		if (indx == -1)
			return 0;

		if (indx != 0)
			detVal = - detVal;

		detVal = detVal * temp.Val[k][k];
		for (i=k+1; i < Row; i++)
		{
			piv = temp.Val[i][k] / temp.Val[k][k];
			for (j=k+1; j < Row; j++)
				temp.Val[i][j] -= piv * temp.Val[k][j];
		}
	}
	return detVal;
}

// calculate the euclidean norm of a vector, for a matrix this does 
// not make sense
Matrix_t Matrix::Norm () const
{
	return sqrt(Norm2());
}

Matrix_t Matrix::Norm2 () const
{
	Matrix_t retVal = Matrix_t(0);

	for (size_t i=0; i < Row; i++)
		for (size_t j=0; j < Col; j++)
			retVal += Val[i][j] * Val[i][j];

	return retVal;
}

Matrix Matrix::ColNorm () const
{
	Matrix retMat(1, Col);
	Matrix_t retVal = Matrix_t(0);

	for (size_t j=0; j < Col; j++) {
		retVal = Matrix_t(0);
		for (size_t i=0; i < Row; i++) {
			retVal += Val[i][j] * Val[i][j];
		}
		retMat.Val[0][j] = sqrt(retVal);
	}

	return retMat;
}

Matrix Matrix::ColNorm2 () const
{
	Matrix retMat(1, Col);
	Matrix_t retVal = Matrix_t(0);

	for (size_t j=0; j < Col; j++) {
		retVal = Matrix_t(0);
		for (size_t i=0; i < Row; i++) {
			retVal += Val[i][j] * Val[i][j];
		}
		retMat.Val[0][j] = retVal;
	}

	return retMat;
}

// calculate the condition number of a Matrix
Matrix_t Matrix::Cond () const
{
	return (Norm() * (!(*this)).Norm() );
}

// calculate the cofactor of a Matrix for a given element
Matrix_t Matrix::Cofact (size_t row, size_t col) const
{
	size_t i,i1,j,j1;

	// make sure its square
	assert(Row == Col);
	// check indecies
	assert(row < Row && col < Col);

	Matrix temp (Row-1,Col-1);

	for (i=i1=0; i < Row; i++)
	{
		if (i == row)
			continue;

		for (j=j1=0; j < Col; j++)
		{
			if (j == col)
				continue;
			temp.Val[i1][j1] = Val[i][j];
			j1++;
		}
		i1++;
	}
	Matrix_t  cof = temp.Det();
	if ((row+col)%2 == 1)
		cof = -cof;

	return cof;
}

// calculate adjoin of a Matrix
Matrix Matrix::Adj () const
{
	// make sure matrix is square
	assert(Row == Col);

	Matrix temp(Row,Col);

	for (size_t i=0; i < Row; i++)
		for (size_t j=0; j < Col; j++)
			temp.Val[j][i] = Cofact(i,j);
	return temp;
}

// Determine if the Matrix is singular
bool Matrix::IsSingular () const
{
	if (Row != Col)
		return false;
	return (Det() == Matrix_t(0));
}

// Determine if the Matrix is diagonal
bool Matrix::IsDiagonal () const
{
	if (Row != Col)
		return false;

	for (size_t i=0; i < Row; i++)
		for (size_t j=0; j < Col; j++)
			if (i != j && Val[i][j] != Matrix_t(0))
				return false;

	return true;
}

// Determine if the Matrix is scalar
bool Matrix::IsScalar () const
{
	if (!IsDiagonal())
		return false;
	Matrix_t v = Val[0][0];
	for (size_t i=1; i < Row; i++)
		if (Val[i][i] != v)
			return false;
	return true;
}

// Determine if the Matrix is a unit Matrix
bool Matrix::IsIdentity () const
{
	if (IsScalar() && Val[0][0] == Matrix_t(1))
		return true;
	return false;
}

// Determine if this is a null Matrix
bool Matrix::IsNull () const
{
	for (size_t i=0; i < Row; i++)
		for (size_t j=0; j < Col; j++)
			if (Val[i][j] != Matrix_t(0))
				return false;
	return true;
}

// Determine if the Matrix is symmetric
bool Matrix::IsSymmetric () const
{
	if (Row != Col)
		return false;
	for (size_t i=0; i < Row; i++)
		for (size_t j=0; j < Col; j++)
			if (Val[i][j] != Val[j][i])
				return false;
	return true;
}

// Determine if the Matrix is skew-symmetric
bool Matrix::IsSkewSymmetric () const
{
	if (Row != Col)
		return false;
	for (size_t i=0; i < Row; i++)
		for (size_t j=0; j < Col; j++)
			if (Val[i][j] != -Val[j][i])
				return false;
	return true;
}

// Determine if the Matrix is upper triangular
bool Matrix::IsUpperTriangular () const
{
	if (Row != Col)
		return false;
	for (size_t i=1; i < Row; i++)
		for (size_t j=0; j < i-1; j++)
			if (Val[i][j] != Matrix_t(0))
				return false;
	return true;
}

// Determine if the Matrix is lower triangular
bool Matrix::IsLowerTriangular () const
{
	if (Row != Col)
		return false;

	for (size_t j=1; j < Col; j++)
		for (size_t i=0; i < j-1; i++)
			if (Val[i][j] != Matrix_t(0))
				return false;

	return true;
}

int Matrix::fillFromFile(const char *file)
{   
	int row=0,col=0;

	ifstream inputFile(file, ios::in);
	if( !inputFile )  {
		//perror("Matrix::fillMatrixFromFile() could not open file");
		return -1;
	} else {
		// first look for two ints, should be a row & col sizes
		if( ! (inputFile >> row >> col) ) {
			inputFile.close();
			return -2;
		}
		// set size	
		this->SetSize(row,col);

		if( !(inputFile >> *this ) ) {
			inputFile.close();
			return -3;
		}

		inputFile.close();
		return true;
	}
}

int Matrix::saveToFile(const char *file)
{
	ofstream outputFile(file, ios::out);
	if( !outputFile )  {
		//perror("Matrix::saveMatrixToFile() could not open file");
		return false;
	} else {
		outputFile << Row << " " << Col << endl;
		outputFile << *this;

		outputFile.close();
		return true;
	}
}

Matrix_t Matrix::Sum() const
{
	Matrix_t sum=0;
	for(size_t i=0; i < Row; i++)
		for(size_t j=0; j < Col; j++)
			sum += Val[i][j];	 

	return sum;
}

Matrix_t Matrix::Max() const
{
	Matrix_t max = -INFINITY;
	for(size_t i=0;  i < Row; i++)
		for(size_t j=0; j < Col; j++)
			max = ( max < Val[i][j] ? Val[i][j] : max);

	return max;
}

int Matrix::MaxInd() const
{
	Matrix_t max = -INFINITY;
	int ind=-1;
	for( size_t i=0;  i < Row; i++)
		for(size_t j=0; j < Col; j++)
		{
			if( max < Val[i][j]) {
				max = Val[i][j];
				ind = i*Col + j;
			}
		}

	return ind;

}

Matrix_t Matrix::Min() const
{
	Matrix_t min = INFINITY;
	for(size_t i=0;  i < Row; i++)
		for(size_t j=0; j < Col; j++)
			min = ( min > Val[i][j] ? Val[i][j] : min);

	return min;

}

int Matrix::MinInd() const
{
	Matrix_t min = INFINITY;
	int ind=-1;
	for(size_t i=0;  i < Row; i++)
		for(size_t j=0; j < Col; j++)
		{
			if( min > Val[i][j]) {
				min = Val[i][j];
				ind = i*Col + j;
			}
		}

	return ind;
}

Matrix_t Matrix::Mean() const
{
	return ( Sum() / Matrix_t(Row * Col) );
}

Matrix_t Matrix::Std() const
{
	Matrix_t sum=0, sumSquared=0;
	Matrix_t num = Matrix_t(Row * Col);
	for(size_t i=0; i < Row; i++)
		for(size_t j=0; j < Col; j++)
		{
			sum += Val[i][j];
			sumSquared += Val[i][j] * Val[i][j];	
		}

	return sqrt( (sumSquared - sum*sum / num) / (num - 1.0) );
}
Matrix Matrix::Skew () 
{
	// make sure its a single vector
	if( Row == 3 && Col == 1) { 
		Matrix_t skew[9] = {         0, -Val[2][0],  Val[1][0],
		                     Val[2][0],          0, -Val[0][0],
		                    -Val[1][0],  Val[0][0],       0 	};

		return Matrix(Row,Row,skew);
	} else if ( Row == 4 && Col == 1) {
		Matrix_t skew[16] = {  Val[3][0],  Val[2][0], -Val[1][0], -Val[0][0], 
                            -Val[2][0],  Val[3][0],  Val[2][0], -Val[1][0],
                             Val[1][0], -Val[0][0],  Val[3][0], -Val[2][0],
                             Val[1][0], -Val[0][0],  Val[2][0],  Val[3][0]};

		return Matrix(Row,Row,skew);
	}

	return Matrix(0,0);
}
Matrix_t Matrix::Trace () const {
	Matrix_t sum = 0;
  for( size_t i=0; i < Row; i++)
    sum += Val[i][i];	

	return sum;
}

// Find the Upper Triangular Sqrt Using Cholesky
Matrix Matrix::Chol_up () const
{
  int8_t i,j,k;
  Matrix_t sum;
  
  // make sure its a square matrix
	assert(Row == Col);
  
  Matrix temp(Row,Col);
  temp.Null();
  
  for (i=0; i < (int8_t)Row; i++)
  {
    sum = 0;
    for (k = 0; k<= (i-1); k++)
      sum = sum + pow(temp.Val[k][i],2);
  
    temp.Val[i][i] = sqrt(Val[i][i] - sum);

    for (j = i+1; j < (int8_t)Row; j++){
      sum = 0;
      for (k = 0; k <= (i-1); k++)
        sum = sum + temp.Val[k][i]*temp.Val[k][j];
      temp.Val[i][j] = (Val[i][j] - sum)/temp.Val[i][i];
    }
  
  }
  return temp;
}

Matrix Matrix::Chol() const 
{
	// make sure its square
	assert(Row == Col);
 
	if( this->Det() <= 0 ) {
		Matrix l(0,0);
		cout << "Matrix::Chol() - matrix must be positive definite" << endl;
		return l;
	}
	
	Matrix L(this->Row,this->Col);
	int i=0, j=0, k=0;
	Matrix_t sumTemp = 0;

	for(i=0; (unsigned)i < Row; i++) {
		// first fill in the diagonal
		sumTemp = 0;
		for(k=0; k < i-2; k++)
			sumTemp += L(i,k)*L(i,k);
			
		L(i,i) = sqrt( Val[i][i] - sumTemp );

		for(j=0; j < i; j++) {
			sumTemp = 0;
			for(k=0; k < j-2; k++)
				sumTemp += L(i,k)*L(j,k);
	
			L(i,j) = 1/L(j,j) * (Val[i][j] - sumTemp);
		}
	}

	return L;
}

Matrix Matrix::Diag() const
{
	size_t m = (Row < Col ? Row : Col);	
	Matrix vec(m,1);
	for(size_t i=0; i < Row; i++)
		vec(i,0) = Val[i][i];

	return vec;
}

void Matrix::Diag(Matrix vec) 
{
	assert( vec.Row == 1 || vec.Col == 1);

	size_t l = vec.length();
	realloc(l,l);

	// zero and set to diag
	for (size_t i=0; i < Row; i++)
		for (size_t j=0; j < Col; j++)
			if( i == j )
				Val[i][j] = vec(i);
			else
				Val[i][j] = Matrix_t(0);

}

Matrix Matrix::Sqrt() const
{
	assert(Row > 0 && Col > 0);
	Matrix tmp(Row,Col);

	for(size_t i=0; i < Row; i++ )
		for(size_t j=0; j < Col; j++ )
			tmp.Val[i][j] = sqrt(Val[i][j]);

	return tmp;
}

Matrix Matrix::mSqrt() const
{
	assert(Row == Col);

  Matrix z(Row,Col), eigValues(Row,Col), temp(*this);
  z.Identity();
  eigValues.Null();

  int i,j,k,l,m,iter;
  Matrix_t d[Row], e[Row];
  Matrix_t scale,hh,h,g,f;
  Matrix_t s,r,p,dd,c,b;

  // First compute the householder transformation to find the tri-diagonal form.
  for (i = Row-1; i > 0; i--){
    l = i-1;
    h = scale = 0.0;
    if(l > 0){
      for (k = 0; k < i; k++)
        scale += fabs(temp.Val[i][k]);
      if(scale == 0.0)
        e[i] = temp.Val[i][l];
      else {
        for (k=0; k<i; k++){
          temp.Val[i][k] /= scale;
          h += temp.Val[i][k]*temp.Val[i][k];
        }
        f = temp.Val[i][l];
        g = (f >= 0.0 ? -sqrt(h) : sqrt(h));
        e[i] = scale*g;
        h -= f*g;
        temp.Val[i][l] = f - g;
        f = 0.0;
        for (j=0; j<i; j++){
          temp.Val[j][i] = temp.Val[i][j]/h;
          g = 0.0;
          for (k=0; k < j+1; k++)
            g += temp.Val[j][k]*temp.Val[i][k];
          for (k=j+1; k < i; k++)
            g += temp.Val[k][j]*temp.Val[i][k];
          e[j] = g/h;
          f += e[j]*temp.Val[i][j];
        }
        hh = f/(h+h);
        for (j=0; j<i; j++){
          f = temp.Val[i][j];
          e[j] = g = e[j] - hh*f;
          for (k=0; k<j+1; k++)
            temp.Val[j][k] -= (f*e[k] + g*temp.Val[i][k]);
        }
      }
    } else
      e[i] = temp.Val[i][l];
    d[i] = h;
  }

  d[0] = 0.0;
  e[0] = 0.0;
  for (i=0; i<Row; i++){
    if (d[i] != 0.0){
      for (j=0; j<i; j++){
        g = 0.0;
        for (k=0; k<i; k++)
          g += temp.Val[i][k]*temp.Val[k][j];
        for (k=0; k<i; k++)
          temp.Val[k][j] -= g*temp.Val[k][i];
      }
    }
    d[i] = temp.Val[i][i];
    temp.Val[i][i] = 1.0;
    for (j=0; j<i; j++) temp.Val[j][i] = temp.Val[i][j] = 0.0;
  }

  // Now use the QL algorithm to find the eigenvalues and vectors of the
  // tri-diagonal form.
  for (i=1;i<Row;i++) e[i-1]=e[i];
	e[Row-1]=0.0;
	for (l=0;l<Row;l++) {
		iter=0;
		do {
			for (m=l;m<Row-1;m++) {
				dd=fabs(d[m])+fabs(d[m+1]);
				if (fabs(e[m]) <= EPS*dd) break;
			}
			if (m != l) {
				if (iter++ == 30) cout << "Too many iterations in tqli" << endl;
				g=(d[l+1]-d[l])/(2.0*e[l]);
				r=pythag(g,1.0);
				g=d[m]-d[l]+e[l]/(g+fabs(r)*g/fabs(g));
				s=c=1.0;
				p=0.0;
				for (i=m-1;i>=l;i--) {
					f=s*e[i];
					b=c*e[i];
					e[i+1]=(r=pythag(f,g));
					if (r == 0.0) {
						d[i+1] -= p;
						e[m]=0.0;
						break;
					}
					s=f/r;
					c=g/r;
					g=d[i+1]-p;
					r=(d[i]-g)*s+2.0*c*b;
					d[i+1]=g+(p=s*r);
					g=c*r-b;
					for (k=0;k<Row;k++) {
						f=z.Val[k][i+1];
						z.Val[k][i+1]=s*z.Val[k][i]+c*f;
						z.Val[k][i]=c*z.Val[k][i]-s*f;
					}
				}
				if (r == 0.0 && i >= l) continue;
				d[l] -= p;
				e[l]=g;
				e[m]=0.0;
			}
		} while (m != l);
	}

  // Convert to eigenvectors of the non tri-diagonal matrix using the orthogonal
  // matrix from the householder transform.
  z = temp*z;

  /* Sort the eigenvalues and vectors
	for (i=0; i<Row-1; i++) {
		Matrix_t p = d[k=i];
		for (j=i; j<Row; j++)
			if (d[j] <= p) p = d[k=j];
		if (k != i) {
			d[k] = d[i];
			d[i] = p;
			for (j=0; j<Row; j++) {
				p = z.Val[j][i];
				z.Val[j][i] = z.Val[j][k];
				z.Val[j][k] = p;
			}
		}
	}*/

  for (i=0; i<Row; i++)
    eigValues.Val[i][i] = sqrt(d[i]);

  // Compute the matrix sqrt from the eigenvalues and vectors
  return z*eigValues*~z;
}

Matrix_t Matrix::pythag(const Matrix_t a, const Matrix_t b) const
{
  Matrix_t absa = fabs(a), absb = fabs(b);
	return (absa > absb ? absa*sqrt(1.0+(absb/absa)*(absb/absa)) :
		(absb == 0.0 ? 0.0 : absb*sqrt(1.0+(absa/absb)*(absa/absb))));
}

